Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Writing a Stiffness Controller (Python)

Downloading the Controller

If you already have already checked out the mit-ros-pkg, you can skip this step. Download the ee_cart_imped stack, which contains the ee_cart_imped_control and ee_cart_imped_action packages, using these instructions.

You now have all the files needed for running the controller and the action server. We will discuss how to compile and run them in the next tutorial.

The Code

First, create a package for this tutorial. Open a new shell, navigate to a directory on your ROS_PACKAGE_PATH in which you wish to do the tutorial and type

roscreate-pkg ee_cart_imped_tutorial actionlib rospy ee_cart_imped_action pr2_controllers_msgs

Within that package, create the src/stiffness_control.py file and paste the following inside it:

   1 #!/usr/bin/env python
   2 import roslib; roslib.load_manifest('ee_cart_imped_tutorial')
   3 import rospy
   4 from ee_cart_imped_action import *
   5 if __name__ == "__main__":
   6 
   7     ## The ee_cart_imped_arm class provides a wrapper around the
   8     ## actionlib.SimpleActionClient class that communicates with the
   9     ## ee-cart_imped_action server. The argument tells it to control the left
  10     ## arm. Setting this argument to False will control the right arm.
  11     arm = ee_cart_imped_arm(True)
  12 
  13 
  14     ## This is a point in the center of the robot's body.  This simply moves
  15     ## the arm to that point with maximum stiffness.
  16     arm.addTrajectoryPoint(0.5, 0, 0, 0, 0, 0, 1,
  17                            1000, 1000, 1000, 30, 30, 30,
  18                            False, False, False, False, False,
  19                            False, 4);
  20 
  21     ## This point is farther in front of the robot, but it is only allowed to
  22     ## use a very small stiffness in the x direction
  23     arm.addTrajectoryPoint(0.5, 0, 0, 0, 0, 0, 1,
  24                            1000, 1000, 1000, 30, 30, 30,
  25                            False, False, False, False, False,
  26                            False, 8);
  27 
  28     ## This is the line that actually sends the trajectory to the action
  29     ## server and starts the arm moving.  The first argument causes
  30     ## the method to block until the trajectory is completed. The
  31     ## second argument tells the method to reset the goal of this arm.
  32     ## Both of these arguments are optional in which
  33     ## case they default to true.
  34     arm.sendGoal(True, True);

Client-Server-Controller Model

Before we examine the code in depth, let's go through the basic outline of the ROS message passing for our controller. The diagram below shows the structure At the bottom level, we have the force/compliance realtime controller from the ee_cart_imped_control package. However, the controller should never be accessed directly at this level because there is no monitoring. For example, if the controller is shut off, there is no way of knowing that. Therefore, all interaction with the controller should be done through the ee_cart_imped_action, which monitors the controller and the trajectory and can abort the trajectory if the controller is no longer responding or the trajectory violates user-defined constraints. The action itself is an action server. To communicate with an action server, an action client is used. You may write your own client to interact with the ee_cart_imped_action, but we have also provided one in the ee_cart_imped_action class. This class is a wrapper around the simple action client and should be sufficient for almost all uses of the controller.

Examining the Code

Now, let's break the code down.

Error: No code_block found

This includes the file that contains the EECartImpedArm class. It, in turn, includes the files for the action server and the controller. Error: No code_block found This is the instantiation of the EECartImpedArm. When instantiating the class, you must pass it the namespace of the action server. This effectively tells the client which arm we want to control. Here, we are controlling the right arm. If we wanted to control the left arm, we would pass in the argument "l_arm_cart_imped_controller". Error: No code_block found The action client sends a ee_cart_imped_control/EECartImpedActionGoal to the server. This instantiates the goal we will eventually send, but first we must specify the trajectory. Error: No code_block found Here, we are filling in a two point trajectory. The EECartImpedArm::addTrajectoryPoint function is a simple function that takes in all of the numbers necessary for a trajectory point, creates the point and adds it as the last point on the trajectory that is passed in as the first argument. Specifically, addTrajectoryPoint(goal, x, y, z, ox, oy, oz, ow, fx, fy, fz, tx, ty, tz, isfx, isfy, isfz, istx, isty, istz, time) adds a point to goal with position (x, y, z) and orientation as the quaternion (ox, oy, oz, ow). The force or stiffness in the x, y, and z directions are given by fx, fy, and fz respectively. The torque or rotational stiffness around the x, y, and z axes are given by tx, ty, and tz respectively. The booleans isfx, isfy, and isfz are true if the arm should exert a force in the x, y, or z directions respectively and false if it should maintain a stiffness. Similarly, the booleans istx, isty, istz are true if the arm should exert a torque around the x, y, or z axes respectively and false if it should maintain a rotational stiffness. The time is the time from the starting time of the trajectory that this point should be reached. For the two points shown here, the PR2 will first move its gripper to approximately the center of its body using maximum stiffness. This move will take 4 seconds. It will then extend the gripper forward, but using only a very small stiffness in the x direction. This move will also take 2 seconds as it is scheduled to complete 6 seconds from the start of the whole trajectory. WARNING: To prevent overdriving the controller, rotational stiffness should never be set above 75. Translational stiffness should never be set above 1000. Error: No code_block found Once the trajectory we have finished creating the trajectory, we send it to the server and start the execution. This line will block until the trajectory completes or is aborted.

Running the Code

Now that we have written a stiffness control example, we need to run the code.